import rclpy                                      # ROS2 Python接口库
from rclpy.node   import Node                     # ROS2 节点类
from rclpy.action import ActionClient             # ROS2 动作服务器类
from move_circle_action.action import MoveCircle    # 自定义接口

class MoveCircleClient(Node):
    def __init__(self, name):
        super().__init__(name)
        self.client = ActionClient(self, MoveCircle, "move_circle")
        self.info = self.get_logger().info
        self.request_future = 0                 # 动作请求回应
        self.action_future = 0                  # 动作执行回应

    def send_action_request(self, enable):
        goal_action_msg = MoveCircle.Goal()     # 请求信息
        goal_action_msg.enable = enable

        self.client.wait_for_server()           # 等待服务器回应
        # 取得服务器对动作请求的回应， 并为其设置对该回应的回调函数
        self.request_future = self.client.send_goal_async(
            goal_action_msg, feedback_callback=self.request_response_callbackk
        )
        # 执行动作之后，节点对服务器的响应消息的回调函数
        self.request_future.add_done_callback(self.action_response_callback)

    def request_response_callbackk(self, feedback_msg):
        '''
            服务器对节点的请求作出回应后，节点执行的指令
        '''
        feedback = feedback_msg.feedback
        self.info('Recevied feedback : %d' % feedback.state)

    def action_response_callback(self, future):
        '''
            服务器执行动作后，节点执行的指令
        '''
        result = future.result()
        if not result.accepted:
            self.info('Goal rejected')
            return
        else:
            self.info('Goal accepted')

        self.action_future = result.get_result_async()
        self.action_future.add_done_callback(self.get_result_feedback)

    def get_result_feedback(self, future):
        '''
            一次动作完成后，节点执行的指令
        '''
        result = future.result().result
        self.info('Result: %d' % result.finished)

def main(args=None):
    rclpy.init(args=args)
    node = MoveCircleClient("action_move_client")
    node.send_action_request(True)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()